/*
 *  Copyright 2021-2025 Michael T. Richter as Half-Baked Software
 *  This program is free software. It comes without any warranty, to the extent
 *  permitted by applicable law. You can redistribute it and/or modify it under
 *  the terms of the Do What The Fuck You Want To Public License, Version 2, as
 *  published by Sam Hocevar. See the file COPYING or http://www.wtfpl.net/ for
 *  more details.
 */

#include <assert.h>

#include "co.h"
#include "co_impl.h"

void co_init_impl(stack_t **stack_pointer, stack_t **stack_canary, co_function co_cb,
                  stack_t *stack_buffer, size_t stack_buffer_count)
{
#define SET_STACK_VAL(OFFSET, VALUE) (stack_buffer[stack_buffer_count - (OFFSET)] = VALUE)
    // check that stack is aligned
    assert((uintptr_t)stack_buffer % 4 == 0);

    // setup stack
    SET_STACK_VAL(1, (uintptr_t)co_prologue_impl); // return to platform prologue
    SET_STACK_VAL(2, 0);                           // r7
    SET_STACK_VAL(3, (uintptr_t)co_halt_impl);     // r6
    SET_STACK_VAL(4, 0xffffffff);                  // r5
    SET_STACK_VAL(5, (uintptr_t)co_cb);            // r4
    SET_STACK_VAL(6, 0);                           // r11
    SET_STACK_VAL(7, 0);                           // r10
    SET_STACK_VAL(8, 0);                           // r9
    SET_STACK_VAL(9, 0);                           // r8

    // setup stack pointer and canary
    *stack_pointer = &stack_buffer[stack_buffer_count - 9];
    *stack_canary = stack_buffer;
}

#if defined(__ICCARM__)
__attribute__((naked)) void co_prologue_impl(void)
{
    __asm volatile(
        "    mov lr, r6 \n" // set lr to point to co_halt_impl
        "    bx r4      \n"
    );
}

#pragma diag_suppress=Pe940
__attribute__((naked)) void *co_yield_impl(stack_t **stack_pointer, void *data)
{
    __asm volatile(
        "    push {r4,r5,r6,r7,lr} \n" // push callee saved registers
        "    mov r4, r8            \n" // yes we need these moves, thumb1 can
        "    mov r5, r9            \n" // only push r0-r7 at the same time
        "    mov r6, r10           \n"
        "    mov r7, r11           \n"
        "    push {r4,r5,r6,r7}    \n"
        "    mov r2, sp            \n" // swap stack, takes several instructions
        "    ldr r3, [r0]          \n" // here because thumb1 can't load/store sp
        "    str r2, [r0]          \n"
        "    mov sp, r3            \n"
        "    mov r0, r1            \n" // return arg
        "    pop {r4,r5,r6,r7}     \n" // pop callee saved registers and return
        "    mov r8, r4            \n"
        "    mov r9, r5            \n"
        "    mov r10, r6           \n"
        "    mov r11, r7           \n"
        "    pop {r4,r5,r6,r7,pc}  \n"
    );
    // We have done the return manually in assembler.
}
#pragma diag_warning=Pe940
#else
#error not compiled with right compiler and architecture
#endif


